﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Windows;
using System.Windows.Media.Media3D;

namespace WiiChuckDemo
{
    public class WiiChuckModel : INotifyPropertyChanged
    {
        private readonly NumericDataSeries[] _series;
        private SerialPort _port;

        public NumericDataSeries AccelerationX { get { return _series[0]; } }
        public NumericDataSeries AccelerationY { get { return _series[1]; } }
        public NumericDataSeries AccelerationZ { get { return _series[2]; } }

        public NumericDataSeries AccelerationXGs { get { return _series[3]; } }
        public NumericDataSeries AccelerationYGs { get { return _series[4]; } }
        public NumericDataSeries AccelerationZGs { get { return _series[5]; } }

        private const int SeriesLength = 500;

        public WiiChuckModel()
        {
            _series = new NumericDataSeries[6];
            for (int i = 0; i < _series.Length; i++)
            {
                _series[i] = new NumericDataSeries(SeriesLength);
            }
        }

        public void OpenSerial()
        {
            if (_port != null && _port.IsOpen)
                throw new InvalidOperationException("Port is already open.");

            _port = new SerialPort("COM3", 19200, Parity.None, 8, StopBits.One); // use settings
            _port.DataReceived += SerialPortDataReceived;
            _port.Encoding = Encoding.ASCII;
            _port.NewLine = "\r\n";
            _port.Open();
        }

        public void Close()
        {
            if (_port != null)
            {
                _port.Close();
                _port = null;
            }
        }

        void SerialPortDataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            while (_port.BytesToRead > 0)
            {
                try
                {
                    DateTimeOffset timeStamp = DateTimeOffset.Now;
                    string line = _port.ReadLine();
                    string[] vals = line.Split(',');

                    for (int i = 0; i < 6; i++)
                    {
                        float value = Int32.Parse(vals[i]);

                        _series[i].AddValue(value, timeStamp);
                    }

                    OnDataRecordReceived();
                }
                catch
                {
                    
                }
            }
        }

        private readonly Vector3D NormalVectorX = new Vector3D(1, 0, 0);
        private readonly Vector3D NormalVectorY = new Vector3D(0, 1, 0);
        private readonly Vector3D NormalVectorZ = new Vector3D(0, 0, 1);

        private double _accelerationLength;
        private double _pitch;
        private double _roll;

        private void OnDataRecordReceived()
        {
            var accelX = AccelerationXGs.CurrentValue.Value/100f;
            var vectX = accelX *NormalVectorX;
            var accelY = AccelerationYGs.CurrentValue.Value/100f;
            var vectY = accelY *NormalVectorY;
            var accelZ = AccelerationZGs.CurrentValue.Value/100f;
            var vectZ = accelZ *NormalVectorZ;

            AccelerationLength = (vectX + vectY + vectZ).Length;

            
            //Roll = Math.Asin(accelX.Length);
            //Pitch = Math.Asin(accelY.Length);
            //Roll = Math.Atan2(accelZ, accelX);
            //Pitch = Math.Atan2(accelZ, accelY);
            var roll = Math.Atan2(accelX, Math.Sqrt(accelY*accelY + accelZ*accelZ));
            var pitch = Math.Atan2(accelY, Math.Sqrt(accelX*accelX + accelZ*accelZ));

            Roll = AngleUtil.RadiansToDegrees(roll);
            Pitch = AngleUtil.RadiansToDegrees(pitch);

            var handler = DataRecordReceived;
            if (handler != null) handler(this, EventArgs.Empty);
        }

        public double Pitch
        {
            get { return _pitch; }
            set
            {
                if (_pitch != value)
                {
                    _pitch = value;
                    NotifyPropertyChanged("Pitch");
                }
            }
        }

        public double Roll
        {
            get { return _roll; }
            set
            {
                if (_roll != value)
                {
                    _roll = value;
                    NotifyPropertyChanged("Roll");
                }
            }
        }

        public double AccelerationLength
        {
            get {
                return _accelerationLength;
            }
            set {
                if (_accelerationLength != value)
                {
                    _accelerationLength = value;
                    NotifyPropertyChanged("AccelerationLength");
                }
            }
        }

        public event EventHandler DataRecordReceived;

        protected void NotifyPropertyChanged(string propertyName)
        {
            var handler = PropertyChanged;
            if (handler != null) handler(this, new PropertyChangedEventArgs(propertyName));
        }

        public event PropertyChangedEventHandler PropertyChanged;
    }

    public static class AngleUtil
    {
        public static double RadiansToDegrees(double radians)
        {
            return ((radians * 180.0) / Math.PI);
        }


        public static double DegreesToRadians(double degrees)
        {
            return ((degrees * Math.PI) / 180.0);
        }
    }

    public class DataSeries<T> : INotifyPropertyChanged
        where T : struct
    {
        private readonly int _maxLength;
        private readonly SynchronizationContext _syncContext;
        private readonly Queue<DataRecord<T>> _values;

        public DataSeries(int maxLength)
        {
            _maxLength = maxLength;
            _syncContext = SynchronizationContext.Current;
            _values = new Queue<DataRecord<T>>(maxLength);
        }


        public event PropertyChangedEventHandler PropertyChanged;

        public DataRecord<T> CurrentValue { get { return _values.LastOrDefault(); } }

        public IEnumerable<DataRecord<T>> Values { get { return _values; } }

        public void AddValue(T value, DateTimeOffset timeStamp)
        {
            AddValue(new DataRecord<T>(value, timeStamp));
        }

        private void AddValue(DataRecord<T> dataRecord)
        {
            _syncContext.Post(new SendOrPostCallback(AddValueCore), dataRecord);
        }

        private void AddValueCore(object state)
        {
            var dataRecord = (DataRecord<T>) state;

            while (_values.Count > _maxLength)
                _values.Dequeue();

            _values.Enqueue(dataRecord);
            OnRecordAdded(dataRecord);
        }

        protected virtual void OnRecordAdded(DataRecord<T> dataRecord)
        {
            NotifyPropertyChanged("CurrentValue");

        }

        protected void NotifyPropertyChanged(string propertyName)
        {
            var handler = PropertyChanged;
            if (handler != null) handler(this, new PropertyChangedEventArgs(propertyName));
        }
    }

    public class DataRecord<T>
    {
        public DataRecord(T value, DateTimeOffset timeStamp)
        {
            Value = value;
            TimeStamp = timeStamp;
        }
        public T Value  { get; private set; }

        public DateTimeOffset TimeStamp { get; private set; }
    }

    public class NumericDataSeries : DataSeries<float>
    {
        private float? _minimum;

        public NumericDataSeries(int maxLength) : base(maxLength)
        { }

        public float? Minimum
        {
            get { return _minimum; }
            set
            {
                if (_minimum != value)
                {
                    _minimum = value;
                    NotifyPropertyChanged("Minimum");
                }
            }
        }

        private float? _maximum;
        public float? Maximum
        {
            get { return _maximum; }
            set
            {
                if (_maximum != value)
                {
                    _maximum = value;
                    NotifyPropertyChanged("Maximum");
                }
            }
        }

        protected override void OnRecordAdded(DataRecord<float> dataRecord)
        {
            base.OnRecordAdded(dataRecord);
            
            Minimum = Minimum.HasValue ? Math.Min(Minimum.Value, dataRecord.Value) : dataRecord.Value;
            Maximum = Maximum.HasValue ? Math.Max(Maximum.Value, dataRecord.Value) : dataRecord.Value;
        }

        public float Average(int count)
        {
            return Values.Reverse().Take(count).Average(p => p.Value);
        }
    }
}
